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Abstract —Accurate estimation of the position of network 
nodes is essential, e.g., in localization, geographic routing, and 
vehicular networks. Unfortunately, typical positioning techniques 
based on ranging or on velocity and angular measurements 
are inherently limited. To overcome the limitations of specific 
positioning techniques, the fusion of multiple and heterogeneous 
sensor information is an appealing strategy. In this paper, we 
investigate the fundamental performance of linear fusion of mul¬ 
tiple measurements of the position of mobile nodes, and propose 
a new distributed recursive position estimator. The Cramer- 
Rao lower bounds for the parametric and a-posteriori cases 
are investigated. The proposed estimator combines information 
coming from ranging, speed, and angular measurements, which is 
jointly fused by a Pareto optimization problem where the mean 
and the variance of the localization error are simultaneously 
minimized. A distinguished feature of the method is that it 
assumes a very simple dynamical model of the mobility and 
therefore it is applicable to a large number of scenarios providing 
good performance. The main challenge is the characterization of 
the statistical information needed to model the Fisher information 
matrix and the Pareto optimization problem. The proposed analy¬ 
sis is validated by Monte Carlo simulations, and the performance 
is compared to several Kalman-based filters, commonly employed 
for localization and sensor fusion. Simulation results show that 
the proposed estimator outperforms the traditional approaches 
that are based on the extended Kalman filter when no assumption 
on the model of motion is used. In such a scenario, better 
performance is achieved by the proposed method, but at the 
price of an increased computational complexity. 

Index Terms —Sensor Fusion, Positioning, Distributed Models, 
Networks, Optimization, Cramer Rao Lower Bound. 

I. Introduction 

In many sensor network applications it is desirable to 
accurately estimate the position of mobile wireless nodes [1], 
Relevant applications are vehicular traffic monitoring, asset 
tracking, process monitoring, and control of autonomous 
agents. As an example, accurate position information is crucial 
for emergency personnel and first responders, see, e.g., [2], 
The commonly used Global Navigation Satellite Systems 
(GNSSs) provide position information with an accuracy of 
about 3 to 10 m in outdoor scenarios. However, in indoor and 
electromagnetically-challenged environments, such as urban 
canyons or forests, the coverage provided by GNSSs is not 
sufficient. Therefore, for such environments, several alternative 
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positioning technologies have been developed, particularly for 
wireless indoor localization; for an extensive review see [3] 
and references therein. 

The topic of localization using wireless signals has received 
much attention in many different research areas. Numerous 
solutions and applications have been proposed, including 
distributed algorithms for wireless sensor networks [4], the 
robust distributed method proposed in [5], and the technique 
for passive device-free sensor localization proposed in [6]. The 
key aspect of cooperation between the nodes of a wireless 
network for localization purposes has been investigated in 
[7], and experimentally evaluated in [8]. The fundamental 
importance of position information for nodes in wireless 
sensor networks is highlighted in [9], where topology-aware 
estimation algorithms are developed, using techniques from 
the spectral graph theory field. In this context, distributed 
gossip algorithms for sensor networks localization have been 
studied in [10] and [11]. In particular, [10] shows that kernel 
averaging techniques for localizing an isotropic source based 
on measurements by distributed sensors may provide im¬ 
proved robustness and accuracy than least squares estimators. 
Moreover, in [11], a distributed algorithm is proposed and 
characterized, which estimates the positions of nodes given 
a minimal number of anchors and using only data-exchange 
between neighboring nodes. 

The robustness with respect to range-measurement outliers 
due to non-line of sight conditions has been analyzed in [12], 
which introduces a robust extended Kalman filter for locating 
a mobile terminal node in wireless networks. Moreover, in 
[13], a Bayesian algorithm is proposed for self-localization 
and tracking of a mobile node through ranging with known- 
position anchors. The algorithm is robust to NLOS propagation 
by combining a Markov model for sight-state estimation 
and a particle filter for location estimation, with a simple 
general motion model. Experiments using the IEEE 802.15.4a 
chirp-spread-spectrum ranging technology show accuracy of 
approximately 2 m in an indoor environment with varying 
NLOS and LOS conditions. In [14], the concept of schedule- 
based network localization is introduced, which enables self¬ 
localization of mobile nodes, as well as localization of the 
entire network, without communication overhead. 

In the context of wireless short range indoor positioning. 
Pulse-based Ultra-Wideband (UWB) techniques are of particu¬ 
lar interest due to numerous good qualities, such as a fine time 
resolution (which allows for a ranging accuracy of the order 
of centimeters when used in conjunction with time-of-arrival 
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methods) a resilience to multipath propagation effects of the 
wireless channel, and a low-power device implementation [15]. 
Recent results on the fundamental limits for wideband radio 
localization of static nodes have been presented in [16]. For 
the case of mobile cellular systems, a performance analysis 
has been presented in [17], The fundamental limitations of 
mobile localization have been extensively studied in [18] for 
bearings-only measurements in the target tracking field, and 
in [19] in the terrain-aided navigation field. In the context 
of sensor networks node localization, the Cramer-Rao lower 
bound (CRLB) has been evaluated for the case of received- 
signal-strength (RSS) range measurements in [20] and in [21]. 
Therein, the RSS model has been linearized with only ranging 
measurements and without using any velocity and orientation 
information. 

To improve the performance of a positioning system in 
terms of reliability of the estimates (integrity), accuracy, and 
availability, it is appealing to process information obtained 
from a number of sensors by means of fusion techniques [22], 
[23]. These techniques involve the processing of different 
information sources to overcome the fundamental limitations 
of sensor measurements such as GNSSs, inertial navigation 
systems (INSs), odometry, and local radio technologies. An 
extensive survey of the most common information sources and 
sensor fusion approaches is provided in [24] in the context 
of automotive positioning. Concerning information fusion for 
UWB, the commonly employed techniques are based on the 
extended Kalman filter (EKF), [25], or other non-linear filter¬ 
ing approaches such as particle filters methods [26]. In [27], 
a 6 degrees-of-freedom tracking system is presented, which 
performs sensor fusion on a UWB distance-measuring device 
and an inertial sensor consisting of triaxial accelerometers and 
gyroscopes. In [28], an UWB sensor fusion technique based 
on the complementary filter approach is presented, where the 
error states are estimated by an EKF using the UWB ranging 
measurements. Subsequently, the estimated errors are fed back 
to correct for the inertial navigation system biases, which 
would otherwise grow unbounded. 

In this paper, we investigate a novel sensor fusion technique, 
based on Pareto optimization, for node self-localization using 
heterogeneous information sources. It is assumed that a node 
wishing to estimate its own position has available ranging, 
speed and orientation information. We stress here that the 
problem that we consider in this paper is self-localization, 
which is fundamentally different from the target tracking 
problem that is widely studied in the signal processing litera¬ 
ture, see e.g. [29]. Furthermore, the fundamental performance 
limitation of the linear fusion is investigated in this paper. 
This investigation is a substantial extension of our earlier 
work in [30] and [31], where the performance of the Pareto 
optimization were not fully investigated and no derivation of 
the CRLB were considered. The posterior CRLB [32] and the 
parametric CRLB [19] are characterized for both any generic 
trajectory of the mobile node and for a specific trajectory. 
Since ranging information gives an error with low bias and 
a high variance, and measurements of the speed and absolute 


orientation (dead-reckoning) give an error with a low variance 
and a high bias, the overall information is jointly processed 
to overcome the individual limitations of ranging, speed and 
orientation measurements. By numerical simulations and by 
evaluating the CRLB, we show that our new method may 
outperform existing solutions, including several commonly 
employed techniques based on the Kalman filter. However, 
the price payed is a higher computational complexity. 

This paper has several distinguished features compared to 
the related work mentioned above. Specifically, our system 
model is original because the mobile node position estima¬ 
tion is performed by ranging measurements, with respect to 
fixed position nodes, together with velocity and orientation 
measurements without any linearization or simplification. For 
the derivation of the estimator, we assume a very simple 
dynamical model of the movement of the mobile node. This 
model allows for flexibility, robustness, low-complexity imple¬ 
mentation and works well in several different scenarios, as we 
show later. Only local processing at such a node is used, thus 
enabling a completely distributed strategy. Furthermore, the 
estimation is performed in a cooperative fashion, in the sense 
that operation of the estimator at the mobile node relies on 
the cooperation of fixed-position nodes acting as responders 
during ranging measurements. 

Therefore, our method is of a general nature and can 
be applied to any motion scenario. Compared to, e.g., [20] 
and [21], we investigate the CRLB without any linearization 
of the ranging model and we consider simultaneously ranging, 
velocity and orientation measurements, which were not con¬ 
sidered therein. Moreover, in this paper, we address one of the 
main challenges for developing sensor fusion methods, namely 
the statistical characterization of the estimation error. Based 
on this characterization, a sensor fusion method is derived 
by solving a Pareto optimization problem, where a tradeoff 
between the variance of the estimation error and its bias is 
exploited. The Pareto optimization approach was first proposed 
in our initial study in this context in [31]. 

The remainder of this paper is organized as follows: The 
problem is formulated in Section II. Then, the proposed sensor 
fusion method is derived in Section III. Furthermore, the 
fundamental limits of an estimator that fuses ranging, velocity 
and orientation measurements are investigated in Section IV 
by means of the CRLB. Numerical simulation results are 
presented in Section V. Finally, conclusions are drawn in 
Section VI. 


A. Notation 


We denote real n-dimensional vectors with lowercase bold¬ 
face letters, such as x, and 111 n x m matrices with uppercase 
boldface letters, such as A. The superscript (-) T indicates 
the transpose of a matrix. Given two matrices A and B, 
the inequality A A B denotes that the matrix B — A is 
positive semidefinite. Given a scalar function /(x) : III n —> IR, 

V x /(x) = 
f(x) : IT 


4f(x) 

dx i 5 

R n , 


d/(x) 

dx. 


-.T 


Vxf(x) = [V/r(x) 


Given a vector function 
we use the gradient matrix definition 
• ■ • V/ n (x)] , which is the transpose 
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Fig. 1. Model of the localization system. The slave nodes are placed at fixed 
and known positions. At time k, the unknown-position master node measures 
its distance ^ with respect to each slave % by measuring the round-trip-time 
of UWB pulses. Speed Vk and orientation 4>k of the master are measured using 
on-board sensors. 


of the Jacobian matrix. We indicate with x a noisy mea¬ 
surement of x and with x an estimator of x. The statistical 
expectation operator is denoted by E {■}. p(x) denotes a 
probability density function (pdf) of the random vector x. 

II. Problem formulation 

Consider a system where a mobile node, which we denote 
as master , needs to estimate its own planar position x^ G R 2 , 
with Xfc = [%\,k %2 ,k] T , where k is the discrete time index. 

To do so, the master measures its distance with respect to 
M devices, which we denote as slaves , with M > 4. We 
do not assume any a-priori information on the mobility of 
the master. It is allowed to move by following a linear 
trajectory with constant velocity as well as a random trajectory 
with random acceleration. An example of such a system is 
provided by the in-house developed experimental platform 
that has been characterized in [28], [33], where the distance 
measurement is obtained by means of the round-trip-time 
of UWB signals. This distance measurement approach, also 
known in the literature as two-way time-of-arrival, avoids the 
need for synchronization between mobile node and slaves, 
therefore allowing for asynchronous operation and reduced 
infrastructure requirements. Alternative approaches, such as 
one-way time-of-arrival and time-difference-of-arrival, suffer 
from clock skew and require additional infrastructure for 
precise clock synchronization [34]. 

We assume that the master is equipped with UWB trans¬ 
mission and sensors that measure its speed and absolute 
orientation. As an example, speed sensors might be imple¬ 
mented by means of odometry in vehicular or mobile-robotics 
applications, such as those modeled in [35]. Furthermore, 
information about absolute orientation could be obtained from 
a heading sensor, for example a compass or magnetometer, 
as in [36]. The diagram in Fig. 1 shows a representation of 
the system model. Furthermore, we assume that the slaves 
are located in fixed and known positions that are denoted as 
Sij, with i = 1,..., M and j = 1, 2. We denote the ranges, 
namely the distances between the master and each slave at 
time instant k, by r\,k, i = 1,..., M. The measurements of 


the ranges, denoted as are affected by additive zero-mean 
Gaussian noise uv,j,fc: 

fi,k = r it k + w r ,i,k ■ (1) 

The variance of depends on the range, according to the 
following exponential model that has been derived from real- 
world round-trip-time UWB measurements in [28]: 

&r,i,k = a o exp (At CT r ijfc ) , (2) 

where cr 2 and k„ are known constants. 

Further, we denote the speed and orientation of the master 
by 14 and cj>k, respectively, whereas 14 is the measurement of 
the speed and r/4 the measurement of the orientation. These 
measurements are expressed as 

Vk = Vk + Wy,k , 4>k = <t>k+ 


where the noise terms wy,k and are zero-mean Gaussian 
random variables with known variances <jy and respec¬ 
tively. 

In this paper, assuming the measurement models described 
above, we propose a novel method to combine and improve 
existing estimators which are well known and commonly used. 
Specifically, we use a so called loosely-coupled approach, 
in which we fuse estimates already available from rang¬ 
ing measurements, with the Weighted Least Squares (WLS) 
method, and from velocity and orientation measurements, by 
performing dead-reckoning. Note that, in order to achieve the 
best possible accuracy, a tightly-coupled approach is necessary, 
where information from the ranging, velocity and orientation 
sensors are fused directly without preprocessing. However, the 
loosely-coupled approach is motivated by the reduced compu¬ 
tational complexity as well as by the increased robustness and 
modularity that it offers. 

This paper has two goals: First, to propose an highly 
accurate estimator of the position of the master by processing 
the results provided by existing ranging-only WLS and dead 
reckoning estimators. Second, to understand the fundamental 
limitations when estimating the position of the master by the 
fusion of information from these estimators. In the remainder 
of this section, we start by formulating the estimation problem 
that leads to the derivation of the proposed estimator. 

The estimate of the master’s position at time k +1 by using 
measurements available at that time is denoted by Xk+i\k+i = 
[^i,fe+t|fc+i *2,fc+i|fc+i ] T ■ We propose that it is derived as 
follows: 


-v ~ (r*) q (v) 

x l,k+l\k+l — C^l,kX l k+1 + ? 

- ~(r) n (v) 

x 2,k+l\k+l — &2,k%2,k-\-l P2,k%2,k+1 ’ 


(3) 

(4) 


where xj^ = 


' ( r ) 
c i,fc+i 


«(r) 

C 2, fc +1 


1 T 


is the position estimate 


based only on the ranging measurements, and x 






i T 


(v) 

k+1 


^l.fc+l X 2,k +1 


is the position estimate based on the dead 
reckoning block, which processes the position estimate at the 
previous time step and the on-board speed and orientation 
sensors. The terms a\ot 2 ,k, Pi,k an d P 2 ,k are the sensor 
fusion design parameters that need to be optimally chosen. 
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Our estimation problem is separable on the X\ and x-2 axes, in 
the sense that the estimates on the X2 axis does not affect the 
X\ axis, and vice versa. Therefore in the following we will 
provide derivations only for the X\ component, because the 
derivations for the X2 component are similar to those for the 
X\ component. 

Typically, the dead reckoning block provides us with an 
estimate having the following expression: 

^i"fe+i = +k\k + VkT cos (j) k . (5) 

Note that we do not make the assumption of a linear motion 
model for the dead-reckoning block. Furthermore, the dead¬ 
reckoning estimate is biased because the orientation measure¬ 
ment appears as the argument of a cosinus. It follows that 
the estimate (3) is biased. This motivates us to model our 
estimator (3) as 

%i,k+i\k+i — £i,fc+i + w Xlk+1 , (6) 

where w Xl k+1 is the error in the position estimate. This error 
has a non zero average, since the dead reckoning block gives 
a biased estimate. 

It is possible to write a simplified form of Equations (3), 
by first noting that 

E {ii,fe+i|fc+i} = a i,k%i,k+i 0i,kXi,k+i 
+ E {w r , k +i} 

+ /3i, fc E {w Vt k+ 1 } + /3i,fcE {w x , k } 

— Xl,k-\-l + {w x ,k-\-l } 

where w r ,k+i and Wv,k+ 1 are the errors in the estimate 
obtained from the ranging block and from the dead-reckoning 
block, respectively. Now, in order to have ai,kXi,k+i + 
Pi,kXi,k+i = 2T,fc+i, it must be ai,k + 0i,k = 1 and therefore 

oti,k = 1 - 01,k ■ (7) 

Hence, by substituting (7) in (3), we obtain 

%l,k+l\k+l — (1 01,k) %1’k+l 01,kX\ k-\-l ’ (^) 

In the following, we will use the simplified expression (8) 
instead of (3). 

From (6) it follows that the error is 

,k+l = Xl,k+l\k+l •t'l,fc+l (9) 

= (1 - 01,k) « 4 i ^ +1 + 01 ,kw xitk 

+ 0i,kTV k cos (j) k - 01 ,kTV k cos (j) k , (10) 

(r) 

where w Xl k+1 is the error of the estimates obtained from 
the ranging. To develop an estimator for the master’s po¬ 
sition, we define a cost function that takes into account 
the estimator variance and bias simultaneously. We de¬ 
fine the bias term of the estimation error as p w i,k+i = 
E {w Xl k+1 } , and the variance term of the estimation er¬ 
ror as + lk+1 = E |(x li fc + 1 |fc +1 — E {$i,fc-|_i|fc+i}) | = 
E |(w IliH1 - E {w Xlifc+1 }) 2 j . 



Fig. 2. Sensor fusion system architecture used at a master node for estimating 
its position at time k + 1 given available information. The dead reckoning 
block gives speed and absolute orientation information, whereas the least 
square block gives ranging information as computed with respect to slave 
nodes. The information provided by these two blocks is then fused to provide 
an accurate estimation *-k+i\k+i- 


We are now ready to formulate a Pareto optimization 
problem to select the fusion coefficients at each time k: 

min pi,k+ w i, k + (! - Pi,k) eh,k C 11 ) 

Pl,k 

s.t. e ^ = [-1,1], pi,k e ^ = [0,1]. 

This problem is motivated by that we would like to reduce 
as much as possible both the average of the estimation error, 
namely the bias due to the dead reckoning block, and the 
variance of the estimation error. The Pareto weighting factor 
(or scalarization coefficient) must be chosen for each time 
k so to trade off the low bias and high variance of the error of 
the ranging estimate with the high bias and low variance of the 
error of the dead-reckoning estimate [37, Section 4.7.5]. Based 
on the statistical properties of the bias, which we characterize 
below, the bias itself may grow unstable with time, which 
motivates the constraint 0i t k £ 3&- The intuition is that the 
minimization of both the average and the variance of the 
estimation error at every time k does not ensure that the bias is 
stable or decreases over time. Notice that in the special case of 
Pi,k = OVfc, the cost function is the variance of the estimation 
error, and when p\^ = 0.5 Vfc the cost function is the mean 
square error (MSE). 

The challenge for such an optimization is the analytical 
characterization of the cost function (11), which we study in 
the following section. 

III. Derivation of a Sensor Fusion Estimator 

The architecture of the proposed estimator is shown in 
Fig. 2. In the following subsections, we provide a statistical 
characterization of the ranging estimate x^.j and the dead- 
reckoning estimate x^.j. Then, in subsection III-D we solve 
the optimization problem, thus achieving the optimal weight¬ 
ing coefficients 0\ t k and 02,k to use in Eq. (8). The original 
contribution of this section is the statistical characterization of 
the estimation errors, and the optimal weighting derivation in 
Proposition 3.7. 
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A. Ranging estimate statistical characterization 


In this subsection we give a new statistical characterization 
of the estimation error of the ranging estimate. We consider the 
problem of finding the first and second order moments of the 
ranging estimates of the X\ and xi components of the position. 
This requires first a derivation of the ranging estimates. In the 
following, for the sake of simple notation, we drop the k + 1 
indices. 

Since it must be that M > 4 for linear-based ranging 
estimates, the master’s position estimate x( r ) may be obtained 
by following a commonly employed strategy (see, e.g., [38]) 
as the solution of Ax* r ) = s, where A and s are easily seen 
to be 


A = 


2 (si,l — Sm,i) 2 (si t 2 — SMg ) 

2 (sM-1,1 — Sm, l) 2 (sm-1,2 — Sm, 2) 


, ( 12 ) 


s= r M — r l) • • • > r M ~~ r M-l 


a + w s 


(13) 


where the constant vector a is given by the following function 
of the known coordinates of the slaves 

s l,l ~ S M, 1 + s l,2 — S M,2 

a = 

s 2 - s 2 4- S 2 - s 2 

L 1,1 1 + *M-1,2 b M ,2 J 

and w s is a random vector given by 

w 2 M + 2 r M w rM - w\ - 2r 1 w ri 

w r M + 2 r M Wr M - w 2 M1 - 2 r M -iwv M _ 1 _ 



(14) 


We derive the ranging estimate by the weighted least squares 
(WLS) technique, since ranging measurements are affected by 
unequal statistical errors across the various links. Then, the 
solution to the following classical WLS optimization prob¬ 
lem gives the ranging estimates: min X ( r ) (s — Ax*'l) , 

where W is a positive-definite weighting matrix introduced 
for the purpose of emphasizing the contribution of those range 
measurements that are deemed to be more reliable [39], and 
||-|| is the Euclidean norm. A common choice for W is R _1 , 
namely the inverse of the noise covariance matrix: 


R = E 


|(w s - E {w s }) (w s - E {w s }) T | . (15) 


Such a choice is motivated by that it would give a maximum 
likelihood estimator in the case of Gaussian errors. Thus the 
well-known solution to the WLS problem is 

x (r ) = (A T WA) _1 A t Ws , (16) 


where W = R 1 . 

We are now ready to introduce the new part of this subsec¬ 
tion. From (16) we can characterize the first and second order 
moments of the error in the ranging estimate. First, however, 
we need the expression of the inverse of the error covariance 
matrix. This inverse matrix has a simple expression and is 
provided by the following lemma: 


Lemma 3.1: Consider the matrix R as defined in (15). The 
matrix has full rank, and the inverse is 


R 1 = I- 


1 


1 + Q 


-Gil 2 D 


where I is the (M — 1) x (M — 1) identity matrix, D is a 
(M — 1) x (M — 1) diagonal matrix whose entries are = 
4r f + 2a i n , l = 1, - •, M- 1, G = pD~ l is a (M-1) x 

M-l 

(M-l) matrix withp = 4r| f c7 2 rM -l-2(T^ r . M ,g= p/T>u, 

2=1 

and 1 is the all ones vector. 

Proof: The lemma follows from tedious algebraic com¬ 
putations, the matrix inversion lemma, and the Woodbury 
identity [39]. ■ 

Based on Eq. (16) and the previous lemma, we are now in 
the position of deriving the expectation and the variance of 
the ranging estimation error w r , defined as w r = x( r ) — x = 

(r) (r)l T 

Wx 1 Wx 2 

Lemma 3.2: The expectation of the ranging estimation 
error is given by: 


E {w r } = (A t WA) 1 A t W 


1 at 


1 T 


J W r . ) ' * ' ) ^ W r 


Proof: See Appendix A. ■ 

Furthermore, we have the following result: 

Lemma 3.3: The correlation of the range estimation error 
is given by 


E {w 2 } = (A t WA) i A T WCW T A(A T WA) 1 , (17) 
where C is a matrix whose diagonal elements are Cu = 


3cr,, 


+ 4 ?mCT 2 


+ 3 at 


with 


+ 4 r, cr 2 — 2 ct 2 a 
1 w n Wr M ~'i 

l = 1 and the oil-diagonal elements are C/ t = 


,K r . + 4 r M (T Wr 


Z°Wr M - 

with l ^ j. 

Proof: See Appendix B. ■ 

This concludes the efforts to characterize the statistics of 
the ranging estimation error. In the following subsection, we 
focus on the statistics of the dead-reckoning estimation. 


B. Dead-reckoning estimate statistical characterization 

In this subsection, we give a new characterization of the 
estimation errors of the dead-reckoning block. We have the 
following results for the first and second order moments of 
the dead-reckoning estimate: 

Lemma 3.4: Let <j>(k) be Gaussian, and assume that V(k) 
and 4>(k) are statistically independent. Then 

E jVfc cos (j)k^ | = Vfe cos (fk) e ^ , (18) 

E j(4 2 cos 2 } = a v Q + \ cos (2 fk) e~ 2a ^ . (19) 

Proof: See Appendix C. ■ 

In the following subsection, we use these results to character¬ 
ize the estimation bias. 

















6 


C. Estimation Bias 


the error bias (20) in Eq. (21) we obtain 


We can now put together the statistical characterization 
of the ranging and dead-reckoning estimations. We have the 
following results that give the terms p w i tk and a^ jl k in 
Eq. (11): 

Lemma 3.5: Consider the estimation error (9). The bias is 

Bwi,k+i = E {w Xl k+1 } = (1 - Pi ,fc) E 
+ Pi,k E {wi 1|t } + Pi, k TV k cos ((j>k) — 1^ . (20) 


Proof: See Appendix D. ■ 

Remark From the previous lemma, we see that the average 
of the estimation error at time k + 1 depends on the bias of 
time k. To avoid an accumulation of the bias, we need to 
impose a condition on Pi <k - Thus, the absolute value of the 
average of the estimation error must be non-expansive, which 
can be easily achieved when |/Si,fc|G [0,1], and thus we have 
& = [—1,1] in the Pareto optimization problem (11). 

Lemma 3.6: Consider the estimation error (9). Then, the 
second order moment is 

E {<* +1 } =Pl k a k + 2Pi <k b k + E {(^t +1 ) 2 } 

( 21 ) 


where 


a k = E |(<) fc+i ) 2 | + E {< J + T 2 E{V fe 2 cos 2 (0 fc )} 

+ T 2 E fe 2 cos 2 (^) (l-2e-^) —2EK lifc }E{ w M +i } 

-2E{ W M fc+| }T14cos(0 fc ) (e~^ - l) 

+ 2I){w xlk }TV k cos((j>k) -l'j , (22) 

bk = -E {« fe+1 ) 2 } + EK,JE{ w W + i } 

+ E {w^ k+i }TV k cos {f k ) (e~^ - l) . (23) 

Proof: The result follows from Lemma 3.2, Eq. (17), and 
Proposition 3.4. ■ 

Finally, to derive an expression for the variance of the 
estimation error cr% !l k that we need in (11), we define the 
variables v r , v Xl and v v as 

W = w xi, k+1 ~ E {^ fc+1 } , v Xl = w Xlk - E {w Xl ' k } , 

v v = T \4 cos (f k ) - T 14 cos (fk) e - ” 2 " . 

Notice that v r , v Xl and v v are independent; we denote their 
variances as <r 2 r , <r 2 x and o 2 v respectively. Also, note that 
E {u r } = E { v Xl } = 0. By substimting the expression for 


oii.fc+i = E { ((1 - Pi,k) w^ k+1 + Pi,kW Xl>k 

+Pi, k TV k cos - Pi, k TV k cos (</> fe ) 

- (1 - Pi t k) E [w^ k+i | - Pi tk E {w Xl k } 

+Pi, k TV k cos (<j) k ) e~~ 

= E |(1 — Pi ik ) vl + Pl^ k v‘l 1 + Pi ik v% 

(1 /^l,fc) Pl,k'^r'^x± 2 (1 (3\f3i,kV r V v 

= (1 ~ Pl,k) &r + Pl,k a x + Pl,k a v i (24) 

where we have used that v r , v Xl and v v are independent and 
E {v r } = E {v Xl } = 0. 

D. Pareto Optimization 

In this section, we put together the results of the previous 
sections to derive a position estimator by solving the opti¬ 
mization problem in (11). Lemmata 3.5 and 3.6 give us the 
analytical expression of the cost function of problem (11). 
Thus, we have the following result, which is one of the core 
contributions of this paper: 

Proposition 3.7: Consider optimization problem (11). Let 
the mean and the variance of the estimation error (9) be 
computed by Lemmata 3.5 and 3.6. Then, the optimal solution 
for a fixed Pareto weighting factor p\, k is 

Pi,k(Pi,k) = max (-1, min (£, 1)) , (25) 

with 

_ 2 (1 - pi.fc) al r - 2 < oi ! fc7E {uy |fc+ i} 

2(1 - Pi,k) V + 2 Pt,fc7 2 

where 

A 2 , 2 I 2 

V — a Vr + a v x + a v v ) 

/ 

7 — ~ E {w r>k+ 1 } + E {w x>k } + TV k cos (<j) k ) ( - 1 

Proof: See Appendix E. ■ 

As a particular case of the previous proposition is given by the 
MSE, when the optimal solution of problem (11) is obtained 
with the Pareto weighting factor pi tk = 0.5 Vfc and results 

Pi, k = max ( -1 ’ min l)) ’ ( 26 ) 

where a k and b k are given by Eq. (22) and Eq. (23), respec¬ 
tively. In general, the optimal value of Pi }k in (25) depends 
on the scalarization coefficient p\^ k (see [37, Section 4.7]). 
The best value of such a coefficient is found by building the 
Pareto trade-off curve and selecting the “knee-point” on this 
curve [37]. Thus, we choose p\ k such that p w i. k and er 2 1 k 
computed in Pl jk {p* hk ) are cr 2 lfc ~ p 2 wl k . This is given by 
the solution to the following further optimization problem 

min (cr 2 1 k (P{ k (pi, fc )) - p 2 wlyk (P{ >k ( Pi,k ))) 2 , (27) 

Pl,k 
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where we have evidenced that p w i t k and cr 2 j k are computed 
in (3* k (pi,fe), according to Eqs. (20) and (24). This problem 
is highly non-linear, but simple and standard numerical pro¬ 
cedures, based on a discrimination of pi t k, can be employed 
to compute approximately the optimal Pareto coefficient p\ k . 
The standard numerical procedure is described in [37, Section 
4.7.5]. Such a procedure consists of first defining a set of 
values that p\ j- can assume, then for each such value the 
procedure does the following steps: 

1) compute /3*{pi t k) via (25) for the current value of pi 

2) use the value of /3*(/Oi,fe) computed in 1) and plug it 
in (20) and in (24) to evaluate p 2 , lk (fii.k (pi,fc)) and 

°ii,k (Pi,k(Phk))-, 

3) use the values p 2 wlk [j3{ k (pi, fc )) and 
a wi,k (Ph (Pi,fc)) computed at step 2) to compute 

Ki , k (Ptk (PI,*)) - plx m (fl.k (Pi,fc))) 2 , (28) 

and store such a value. 

The approximate optimum p\ k is the one which corresponds 
to the minimum of the stored values of (28). Such a pik cor¬ 
responds to the "knee point" of the Pareto trade off curve [37, 
Section 4.7.5]. 

Finally, recall that the values of the optimal /3 2< k and 
p x2 k yielding the x 2 component of the position estimate are 
obtained similarly to what done for the x\ component. 

IV. Cramer-Rao Lower Bound 

In this section, we are interested in investigating the funda¬ 
mental performance limitation of our estimator proposed in the 
previous section. To do so, given any estimation problem, the 
CRLB is a fundamental tool for performance analysis, since it 
quantifies the best achievable mean-square error performance 
of estimators [39]. We wish to derive such a bound to 
compare it to the performance of our estimator. There are two 
fundamental approaches: the parametric CRLB and posterior 
CRLB. 

The parametric CRLB, that we denote as RnrCRLB, is the 
CRLB evaluated for a given state-space trajectory [19]. There¬ 
fore it is equivalent to the problem of estimating unknown 
deterministic states considering a zero process noise in the 
dynamical system model; for the proof see, e.g., the derivations 
in [40]. In other words, the state is treated as a deterministic 
but unknown parameter for the purpose of the evaluation of the 
Par CRLB. Therefore, the Par CRLB is simple to evaluate in a 
numerical simulation environment where the true trajectory is 
known, because the equations required for its calculation do 
not contain any statistical expectation operator; see, e. g., [41]. 
However, due to its nature, the ParCRLB does not provide 
information about the fundamental performance limitations in 
the general case. For such a case, we need to investigate the 
posterior CRLB, which we do in the following. 

A. Posterior CRLB 

For recursive estimation problems in the Bayesian frame¬ 
work, where the state is treated as a random variable, the 


bound is denoted as the posterior CRLB (PCRLB). A recursive 
formulation of the PCRLB has been first introduced in [32]. 
In the following, we derive such a bound for our localization 
problem. The core contribution of this section is summarized 
in Proposition 4.5 below. 

Differently from the case of the ParCRLB, where one is 
interested in calculating the performance bound for a given 
trajectory, here we aim at characterizing the fundamental 
performance limits, on the average, for all the trajectories 
belonging to a class, which is defined by a dynamical model. 
Therefore we introduce the state € R 1 to model the bi- 
dimensional position of the master, its absolute speed and 
its orientation, that is = \%x,k x 2 ,k V k 4>k] , and we 

consider the following non-linear state-space model: 

Pfc+i = f (p k) + v fe (29) 

y fc = h(p fc )+e fc , (30) 

where v fc = [v ltk v 2 ,k « 3 ,fc f 4 ,fc] T S 1R 4 ~ 

7V(0,Q) is the white Gaussian process noise with Q = 
diag (a 2 , tr|, <r§, ct|), where it is natural to assume that 
(7 1 = (72, namely that the process noise on the two coordinates 
has same variance, and e R M+2 ~ Af (0, Rk) is the white 
Gaussian measurement noise, which is independent of v*,, 
with R k = diag (ct 2 lfe ,..., cr 2 M k , a 2 V , af ). Simple algebra 
gives that the state update and measurement functions are, 
respectively, 

%i,k + TVk cos (j>k 

f , \ X 2 ,k + TVk sin (f) k 

1 (Pfc) = T/ > 

Vk 

4>k 

h (pfc) = [ hi, ..., hM, hM+ 1, h,M +2 ] 

sj (s M - Xi ,fc) 2 + (si,2 - x 2: k) 2 


\J (Stf, 1 - 2T,fc) 2 + (SM,2 - Z2,fc) 2 ’ 

V k 

ftk 

where we recall that T is the sampling interval. 

Consider the dynamical system of (29) and (30), and denote 
as P/, the covariance matrix of any unbiased estimator of the 
system’s state at time k. Then, P k is lower bounded by the 
inverse of the Fisher information matrix .R, that is J A . 1 A P fe . 
Such a matrix can be recursively computed as [32] 

J fe+1 =D 22 - D 21 (J fc + D 41 )- 1 D 42 (31) 

with 

D 44 = ^ E |v xfe [V Xfc logp(x fc+1 |x fc )] T } , (32) 

D 21 =(D 42 ) T = -E {v Xfc [V Xfc+1 logp ( X fc +1 |xfc)] T | , 

(33) 

D fe 2 =- E {Vx H1 [V Xfc+1 logp (xfc_|_i| X fc)] T | 

- E | V Xfc+1 [V Xfc+1 logp (yfc +1 | X fc)] T | , (34) 

where the initial condition is given by the information matrix 
Jo, which is computed based on the initial prior density p (xq). 








The system model in (29) and (30) has additive Gaussian 
noise. Hence, the terms in (32), (33) and (34) may be ex¬ 
pressed as (see [32, eq. (34) - (36)]) 

Dfc 1 = E {[V Pfc f( P fc)] Q- 1 [V Pfc f(p fc )] T } , (35) 

Di 2 = -E{[V Pfc f(p fe )]}Q-\ (36) 

Df = Q" 1 + E j[V Pfe h( P fc)] R k _1 [V Pfc h( P fc)] T } . 

(37) 


In general, there is no guarantee that the expectations in (35)- 
(37) exist. In other words, the CRLB may not exist. Therefore, 
in the following we establish the existence of the CRLB 
by deriving closed form expressions for the first two terms, 
namely Djf, 1 and D{ 2 , while we derive bounds for the third 
term D| 2 . We start by proving the following useful Lemma: 

Lemma 4.1: Consider the dynamical system in (29) - (30), 
denote the speed and the orientation at time k = 0 as 
Vo and 0o, respectively, and define the following quanti¬ 
ties: cq = cos ^ 0 ; so = sin 00 ; = cos20o, s' 0 = 


sin 20o, £k — e 


(k-l)o-j 


Then 


E {V k } = V 0 , 

E {cOS 0/c} = c 0 £ k , 

E {sin 0fc cos 0fc} = ^s' 0 £ k , 


E {V k } = (k - 1) a 2 + Vq 
E {sin 0fe} = s 0 £ k 

E {cos 2 0 fc } = i + ^c' 0 4 


Proof: See Appendix F. ■ 

Then, we note that the gradient of f is easily evaluated as 


V P J(Pfc) 


1 0 0 0 
0 10 0 
T cos 0fc T sin <p k 10 

-TV k sin0 fe TV k cos <p k 0 1 


(38) 


By substituting Eq. (38) in Eq.(35) and using Lemma 4.1 we 
obtain Eq. (39) (see top of next page). Note that, when k tends 
to infinity, this matrix tends to a diagonal matrix, because £ k 
tends to zero, but the right-lower element tends to infinity. 
This implies that, as k grows, the most relevant contribution 
to the Fisher information matrix in Eq. (31) is given by the 
D 22 term. Therefore, in the limit case, only the measurements 
influence the PCRLB, not the model. 

Similarly, by substituting Eq. (38) in Eq.(36), we obtain: 

af 2 0 0 0' 

0 erf 2 0 0 

Tcrf 2 c 0 £fe Taf 2 so£ k erf 2 0 

-Terf 2 Vbs 0 £fe Taf 2 V 0 c 0 £ k 0 erf 2 



We now turn our attention to the D 22 term in (31). For 
the sake of simple notation, we drop the k subscript in the 
remainder of this section and we define the following quantity: 


7 _A 

a i,j — 




\j(x\ - Sj,l ) 2 + (x 2 



After some algebra, the gradient of h is 


V p h(p) 4 


<7i,i 

^2,1 

0 

0 


di,M 0 0 

d,2,M 0 0 

0 1 0 

0 0 1 


Let us introduce the following definition for the matrix II in 
Eq. (40) (see top of next page). By substituting Eq. (41) in 
Eq. (37), and using the above definitions, we obtain 


D 22 = Q 1 + 


n o 
0 Rf 1 J ’ 


(42) 


where 0 is the 2x2 zero matrix and R 2 is defined as the 
2x2 right-lower block of the measurement noise covariance 
matrix, that is R 2 = diag [cry , . 

In Eq. (42), there are two main issues: first, one has to 
show that the expectations in the submatrix II exist, and 
then one has to compute them. These expectations are taken 
over non-linear functions of the state random variables, which 
makes it hard to find a closed-form expression of the PCRLB. 
This is a well known issue, as pointed out in [42], where 
after arguing the difficulty of deriving a PCRLB in cartesian 
coordinates, as we do, the PCRLB has been derived by 
using logarithmic polar coordinates. However, we note that 
the model adopted in [42] does not not include speed and 
orientation measurements by on-board sensors, and that the 
derivation of the PCRLB in a coordinate system does not give 
insight on which is the best estimator on another coordinate 
system [37], [41], which greatly limits the model of [42] for 
our case. In the cartesian coordinate system one could resolve 
the issue by a Monte Carlo approach to calculate numerically 
the PCRLB. However, analytical expressions of the PCRLB 
are of fundamental interest for many localization applications, 
such as when trying to understand where to place the slaves 
in order to maximize the information content given by the 
Fisher information matrix [43], [44], or when planning the 
path of a robot to minimize the uncertainty in its location. 
This further motivates the derivation of the analytical results 
that we present below. 

We must first prove that the expectations in the submatrix 
n exist. In the following, we establish the existence of such 
expectations by deriving them in closed-form, and we derive 
closed-form expressions of upper and lower bounds for the 
entries of II for which the exact closed form expression is 
too complex for practical numerical evaluations. Then, we 
calculate them numerically in Section V. To compute the 
expectations on the diagonal elements of II, we establish the 
following result. 

Proposition 4.2: Let q and 2 be independent Gaussian 
random variables with average and standard deviation // (J , cr q 
and fi z and a z , respectively, with o z = o q 1 . Then, 


E 


2 . 2 

q + z 


1 

1 + T 


k =1 


/*F,k 

(1 + T) k 


(43) 


where 


-y A Pi + 

1 2,2 

Pq + °n 


1 + 


fc= 1 


("if 


P 2 q k Pk 

{pl+OqY- 


and /Zfc denotes the /,:-th central moment of the noncentral 
chi-squared distribution of 1 degree of freedom, and jiF.k 


1 Recall that these are the variances of the process noise on the coordinates, 
see Eq.(29). 


(41) 
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-r 2 

0 

-77 cos (j) k 

-7714 sin <j) k 

\ 


0 

- 2 “ 2 

77 sin (j) k 

zpVk COS <f) k 

/ 2 

T 2 V k cos f k sin f k 



COS (f>k 

°i 

77 sin f k 
°2 

nr 2 0 . rp 2 . 9 / _2 

~^r cos (pk + -yr sin (pk cr 3 



— -72-14 sin (f> k 
°1 

It 14 COS f k 
°2 

T 2 Vfc cos f k sill f k 

^V 2 sin 2 f k + ^V 2 cos 2 f k + erf 2 

> 


“1 

0 

-^e k 


—2 


^ £ k 
°2 


T‘ 


/£r+£2 

V 


Tcq 


£k 




~2aTa\ 


±r' 

J C 0 £ , 


rji 2 ( a l ~ a 2 \ ^ 0 5 0 
T -^ e 


,^ 4 )_ 
4 
k 


-2 


TVqsq 

- ^~ £k 

TVncn , 
—T5—£fc 


T 2 { a *7 a } 

\ 2 a i ,J 2 . 

T 2 (k - 1) of (jppfr + ^^f c o £ t) + ^4 ' 


■ (39) 


n = E 


o- ri 2 di, 1^2,1 


^"Xi 


(T~ 2 d 2 
<T r M u l,M 

nf 2 d l.Mtfe.M 


v ri 2 di,id 2 ,i 

'~ 2 d 2 


cr 


n “2,1 


^ r 2 di t Md 2 ,M 
~ 2 d 2 


cr. 


tm Uj 2,M 


(40) 


denotes the fc-th central moment of the doubly noncentral F- 
distribution. 

Proof: See Appendix G. ■ 

The previous proposition allows us to establish the exis¬ 
tence of the expectations on the diagonal elements of II. A 
similar result can be derived for the off-diagonal elements. 
Unfortunately, these closed form expressions are of limited 
practical usage due to the computational complexity of the 
many summations and function evaluations therein involved. 
Accordingly, we derive the following upper and lower bounds, 
which are much more easy to use in practice: 

Proposition 4.3: Let q and z be independent Gaussian 
random variables of average and standard deviation ji q , o q 
and fj, z and a z , respectively. Then 


a a—\n {^l+k-q+ a z+ a l) < 



< 1 , 


where a = ~7 e - In (2) - 2 In (cr q ) - M (0, 1/2, -/ i 2 /2a 2 ) , 

the symbol denotes the Euler Gamma constant, and the 
symbol M(a, b , z ) denotes Kummer’s confluent hypergeomet¬ 
ric function [45]. 

Proof: See Appendix H. ■ 

To compute bounds on the off-diagonal elements of II, we 
give the following proposition: 

Proposition 4.4: Let q and z be independent Gaussian 
random variables with average and standard deviation // f/ , a q , 
and n z and cr z , respectively. Let the expectation be computed 
in the Cauchy principal value sense. Then 



Proof: The bounds follow immediately from that —1/2 < 
qz/(q 2 + z 2 ) < 1/2, Mq G R, Mz £ R. ■ 

We are now in the position to compute the upper and 
lower bounds on the Fisher information matrix, in the positive 
semidefinite sense, by the following result. 

Proposition 4.5: Let ,l (vh > and .l nh) be two matrices con¬ 
sisting of element-wise upper and lower bounds, respectively, 
of the Fisher information matrix J in Eq. (31) as obtained by 


Propositions 4.3 and 4.4 for the elements of II of Eq. (42). 
Let 


T ( ub ) 


j(ub,G) A. 


if jf b) > min (uj 


(row) (col) 


. ( (row) (col) , 

nnn I ul , u\ ) + e 


if j'/' b) < min 


• ( (row) (col)\ 


(44) 

(45) 

(46) 


j(“ b ’ G) =jf b) , V i, j = 1... n , with i^j, (45) 

Vi = 1... n, 

' J [f if Jj| 6) > min (zf ow) , l^) 
jg b ’ G) 4 (min (j ^ ] /l ( r w) , jW/lW) - e) J 


(lb) 

ij 


if } < min ( l ^ row) , if° l) 
Vi,j = l...n, with i f j , 


(47) 


, (row) \ ' I T (t 

where u- = > JL 


i ( ub ) 

, V 

y^ \j(ib) i\coi) _ y^ I j(/6) 
/ ^ | ij ’ i / | ji 

j^i j^i 

with e > 0. Then 


, l 


(row) 


(col) _ T (“U 

’ U i ~ 2^ | 
jV* 

and e is an arbitrary constant 


0 -< j(lb,G) ^ J x j(«b,G) _ 


(48) 


Proof: See Appendix I. ■ 

This concludes the steps to establish the existence and 
upper and lower bounds of the Fisher information matrix. 
In the following section, we use these results to compare 
the performance of our proposed localization method to the 
CRLB. 


V. Simulation Results 

In this section we present simulation results to validate 
both the PCRLB derivations and the estimator we presented 
in the previous sections. In particular, we compare our new 
estimator based on Pareto optimization that we have developed 
in Section III to some solutions from the literature and to the 
PCRLB we derived in Section IV. 
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A. Performance of the proposed sensor fusion method 

In this section we present extensive simulation results of our 
new estimator ( 8 ) based on the optimal fusion coefficient of 
Eq. (25) and Pareto weighting factor of Eq. (27). The values 
of the parameters used in the numerical simulations are the 
following: 

• Speed measurement noise standard deviation cr v = 
0.05 m/s, to model the worst-case performance of an 
odometry sensor, [35]; 

• Orientation measurement noise standard deviation a$ = 
7t/ 8 rad, to model the worst-case performance of a mag¬ 
netometer subject to disturbances due to the environment, 
see, e.g., [36], [46]; 

• Ranging noise model parameters (in Eq. (2)): <to = 0.25, 
K a = 0.25; 

• Sampling time interval T = 0.1 s. 

The following two scenarios have been considered: 

(A) Linear trajectory with constant speed of 0.1 m/s. This is a 
deterministic trajectory, that is, no process noise is added. 
As an example, this scenario emulates the real-world 
situation in which the mobile node is on a vehicle moving 
along a straight line without performing any maneuver. 

(B) Piece-wise-linear (PWL)-acceleration trajectory, which is 
more complex and realistic than the linear one, with 
a maximum acceleration of 0.5 m/s 2 , see Fig. 5. This 
scenario emulates a mobile node performing maneuvers 
with an acceleration range consistent with the indoor 
environment, e.g. a walking person carrying the mobile 
node. 

The absolute value of /?*, provided by the algorithm, 
has been clipped to 0.99 to ensure that the average of the 
estimation error is non-expansive, as discussed in Section III. 

Note that, in 3.7, in the expression for 7 the true speed 
14 and orientation 7 a- terms are present. Since these are not 
available, we use these approximations: 

Eapprox (k) — — (^{f-l,k\k l|fc—l) 

1 

+ (%2,k\k ~ ^ 2 ,fe-l|fc-l) ) ) (49) 

I / 7 \ , f &2,k\k — ^2,fe—l|fe —1 \ tens 

</>ap P rox (k) ~ arctan -- 7 - . (50) 

%l,k—l\k— 1 / 

Also, note that the second-order moment of the ranging error, 
given by (17), depends on the true ranges r ,. Therefore, for 

the calculation of the term E | in bk and at, 

we use the approximations for the ranges 

4 (a fc p +i° X) = \j ( s fi _ Zkf + (s,, 2 - y k ) 2 , i = 1,... ,M. 

(51) 


The absolute error and empirical Cumulative Distribution 
Function (CDF) obtained by our new method for scenario A 
are shown in Fig. 3 and 4. In this scenario, a root-mean- 
squared error (RMSE) of 4.0 cm in the position estimate 
over the entire trajectory has been obtained by our method. 
Furthermore, in Fig. 5 the more realistic PWL-acceleration 
case is shown, where an RMSE of 5.5 cm is obtained. 




10 20 30 40 50 60 

Time [s] 


. ranging error 

- * - dead reckoning error 


Fig. 3. Absolute errors in scenario A. The dead-reckoning estimates, 
asterisks, have a high bias, while the WLS ranging estimates, dashed line, 
present a higher variance, but a reduced bias. The proposed sensor fusion 
technique is able to reach a good tradeoff between estimator bias and variance. 



Absolute position error [m] 


Fig. 4. Empirical CDF of the absolute positioning error in 2D for scenario A. 
Using the proposed method (blue line), about 95% of the position estimates 
have an error smaller than 7 cm, and the performance is improved with respect 
to both the ranging and the dead-reckoning stand-alone systems. 


Numerical simulation results show that the approximations 
(49)-(51) give good performance and that the proposed method 
yields a good trade-off between variance and bias of the esti¬ 
mated position. Therefore, the proposed estimator overcomes 
the limitation of dead reckoning (that is the slowly accumu¬ 
lating bias) while also reducing the relatively higher variance 
of the ranging estimator. The result is an overall smooth 
and accurate estimate of the trajectory. In the following, we 
compare the proposed estimator to other solutions that are 
commonly adopted in similar localization problems. 


B. Performance Comparison 

Here we compare the sensor fusion based on Pareto op¬ 
timization proposed in this paper to several other methods 
in the two mobility scenarios considered in the previous 
section. To provide an extensive comparison, the simulations 
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Fig. 5. PWL-acceleration trajectory case, scenario B. The proposed sensor 
fusion technique closely tracks the true trajectory with RMSE = 5.5 cm. 

Table I 

Average Execution Time Per Iteration [s]. 

Matlab® implementation on an Intel® Core2 Duo, 


2.40 GHz PC with 3 GB RAM. 


EKF 

1.3 x 1(T 4 

MSE Eq. (26) 

1.7 x 10 -4 

LC-KF 

1.9 x 10 -4 

UKF 

4.2 x 10“ 4 

Proposed sensor fusion, Eq. (25) 

4.5 x 10“ 4 


are performed for different values of the sampling period T 
and of the speed V for scenario A, whereas for scenario B the 
simulations are performed for different values of T and of the 
maximum acceleration. Moreover, for each configuration, the 
simulations are performed for 10 realizations of the random 
noises and the resulting RMSE values are averaged. 

The sensor fusion methods available from the literature that 
we have considered are the following: 

• Extended Kalman Filter (EKF) [25], where the measured 
speed and orientation data are employed as inputs, and the 
ranges as measurements. No motion model is assumed. 

• Unscented Kalman Filter (UKF) [47], with the same 
state-space model as the EKF above. 

• Loosely Coupled Kalman Filter (LC-KF), where range 
measurements are preprocessed with the WLS algorithm 
to obtain preliminary position measurements, thus using 
a linear measurement equation. The usual linearization of 
the state function is performed for calculating the input 
noise matrix [25]. 

Notice that the methods based on the Kalman filter theory 
have been implemented without assumptions about the motion 
model, to provide a fair comparison with the developed method 
of this paper. If prior information about motion models is 
included in the state space formulation, the Kalman approaches 
might be able to provide better performance, as we will 
show in the example in the next section. Notice also that we 
provide results of the MSE special case when px t k = 0.5 
Vfc in Proposition 3.7. This method does not provide good 



Fig. 6. RMSE vs speed of the different methods for Scenario A with T = 0.5 s. 



Fig. 7. RMSE vs maximum acceleration of several methods for Scenario B 
with T = 0.1 s. 

performance compared to the more general case of choosing 
the best p-j j- by problem (27). However, it features a much 
smaller computational complexity. 

Comparisons for two selected simulation configurations are 
provided in Fig. 6 and 7, as obtained in the two scenarios 
with different values of T. The proposed sensor fusion based 
on Pareto optimization outperforms the other methods, while 
the KF-based methods have similar performance among them. 
However, the proposed sensor fusion based on Pareto opti¬ 
mization requires the highest computational load among the 
implemented techniques, as shown in the average simulation 
execution times listed in Table I, although it is close to the 
execution time of UKF. 

C. CRLB evaluation 

In this section, we provide a comparison of the RMSE 
performance of the proposed sensor fusion method based on 
Pareto optimization and the Kalman filter based methods we 
used in the previous section to the fundamental limits given 
by the Cramer Rao lower bound. In particular, we first provide 
comparisons with the ParCRLB, and then with the PCRLB as 
derived in Section IV. 

The performance of the filters was compared based on the 
RMSE of the position estimate, where the error is defined 
as the Euclidean distance between the true position and the 
estimate. The results compared to the ParCRLB for several 
specific trajectories are presented in Fig. 8, where the error 
curves were obtained by averaging over 100 Monte-Carlo runs. 
We notice that the proposed sensor fusion method based on 
Pareto optimization provides better performance than the other 
considered KF methods as it is the closest to the Pc/rCRLB. We 
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-ParCRLB —*— Proposed fusion —I— Special case (MSE) —b— LC-KF —©— EKF —*— UKF 





Fig. 8. Error performance of the proposed sensor fusion method based on Pareto optimization of Eq. (25) and other methods against the theoretical bound 
given by the square root of the trace of the Pc/rCRLB (dashed line): (a) linear trajectory, constant velocity of 0.5 [m/s]; (b) linear trajectory, constant velocity 
of 1 [m/s]; (c) spline trajectory, maximum acceleration of 0.3 [m/s 2 ]; (d) spline, maximum acceleration of 1 [m/s 2 ]. 
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Fig. 9. Numerical simulation results on the class of trajectories defined by the 
model in (29)-(30). The behavior of the proposed sensor fusion method based 
on Pareto optimization is compared to other estimators from the literature and 
to the PCRLB derived in Section IV. Error curves are obtained as the average 
of 1000 Monte Carlo iterations. 


stress that none of the considered methods makes assumptions 
about the motion model, therefore in this sense the ParCRLB 
is an overly optimistic bound. 

Furthermore, the numerical simulation results related to the 
model in (29)-(30) are shown in Fig. 9, in comparison with 
the derived PCRLB in (31). The method denoted as “EKF 
CV” is implemented using the motion model in (29), and the 
same model is used to generate the trajectories. Specifically, 
a standard EKF is implemented [25], by linearizing about 
the current estimate of the mean and covariance, where the 
Jacobian matrices are computed by numerical differentiation 
at each iteration. In this case, we notice that EKF CV out¬ 
performs the proposed sensor fusion method based on Pareto 
optimization and approaches the PCRLB. This confirms that 
the knowledge of the specific underlying motion model is 
beneficial for estimation. However, the proposed sensor fusion 
method still provides good performance, improving over the 
LC-KF. Conversely to EKF CV, the proposed method does not 
assume any specific motion model, thus it can be applied to a 
wider class of trajectories, proving to be robust with respect to 
model mismatch. Nevertheless, the same Pareto optimization 
approach presented in this paper could be applied to derive an 
estimator which exploits the knowledge of the motion model, 
although such derivation is outside of the scope of this paper. 


VI. Conclusion 

In this paper, the fundamental limitations of position esti¬ 
mation for a mobile node by the fusion of information from 
ranging, velocity and angular measurements were investigated. 
Upper and lower bounds of the posterior Cramer-Rao lower 
bound were derived. A sensor fusion method based on Pareto 
optimization was proposed. The analysis was validated by 
Monte Carlo simulations, which showed that the proposed 
method provides better performance than Kalman filtering- 
based methods when no knowledge of the dynamic model of 
the underlying system is assumed or when the model is time 
varying. 


Future work includes the extension of the proposed esti¬ 
mator to cases when the mobility model is known. The good 
performance achieved by the proposed sensor fusion method 
based on Pareto optimization in the case of no motion model 
assumption lets us think that the use of a motion model would 
substantially farther improve the performance of the proposed 
method. 


Appendix A 
Proof of Lemma 3.2 

By taking the mean of Eq. (16), we obtain: 

E jx (r) j =(A t WA) - 1 A t W 

x (lE {r 2 M }~ E {[f?,...,r : M_!] T }+a) 

=x + (a t wa) _ 1 A t W 

X ( 1(T ^m - 

=x + E {w r } , 
whereby the lemma follows. 

Appendix B 
Proof of Lemma 3.3 

By taking the square of Eq. (1) we get rf = rf + w 2 . + 
2 TiW ri . Then, using this expression in (14) and substituting in 
(16), we have the following expression for the ranging-based 
estimate: 

X«=(A T WA)' 1 A T W{[ rl i ~rl...y M -r 2 M _ l ] T 
+Bfc + a} , 



where 


Bfc = 


w 2 M + 2 r M w rM - w\ - 2r 1 w ri 
w 2 rM + 2 r M w rM - w 2 TM _ x - 2r M -iw rM _ 1 


Therefore the error can be expressed as w r = 
(A t WA) 1 A i WB / t, and its correlation is E {w,^} = 
(A t WA) _ 1 A t WE {B fc B[}W T A(A T WA)' 1 . We let 
C = E {BfcB^}, whereas the lemma follows after some 
algebraic manipulation. 


Appendix C 
Proof of Lemma 3.4 

Since 14 and </>£ are independent, 

E |Vk cos (0*)} = E { 14 } E { COS «} 

= 14 E {cos(</>fc + w^)} 

=14 (cos(</>fc) E {cos(wj 0 jfc )} 

-sin(0 fc )E {sin(io^fc)}) . (52) 
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By using the cosine series, 


w l,k , w t,k 




E {cos(w^ k )} = E|l-^ + ^ + ... + (-1)"^T 


= 1 - 


O'? crl 


2 2 2 • 2 ! 


+---+(- 1 ) n ^rS = e_ E 

2 n • n! 


(53) 


where we have used that <j> k is Gaussian. Similarly, by using 
the sine series, it results 


E {sinO^fc)} = E < - -j- + 


^^ k ^(fi,k p,k 


3! 5! 7! 


+ --- + (-iy 


< +1 ' 

(2n+l)! 


= 0. (54) 


Then Eq. (18) follows by substituting Eqs. (53) and (54) in 
Eq. (52). Furthermore, since V k and &k are independent, we 
have 

E |l4 2 cos 2 (^)} = E {y 2 } E {cos 2 (<^)} . (55) 


Appendix D 
Proof of Lemma 3.5 

The expectation of ( 8 ) is 

E{x ljfc+1 | fc+1 } = (1 - 0i,k)x k +i + (1 ~0i,k) E |w^ fc+1 | 
+ 01 ,fc f Xfe + E {w Xl k } + TVk cos ((j>k) e~~ 

= (1 - Pi,k) %k+ i + 0i, k x k +i + (1 - Pi,k) E |w^ fe+1 } 

+ 0i t h E{tu a;iife } + 0i, k TV k cos (<j> k ) _1 

= x fc+ i + E{iu Xli£+1 }. 

whereby the lemma follows. 

Appendix E 

Proof of Proposition 3.7 

The cost function in (11) is always positive for any choice 
of 0 i : k € 1 R. and of p\ k , and it is a parabola in 0 i, k , which 
is due to that a k > 0. It follows that the cost function is 
convex. Therefore, the optimal solution is given by computing 
the derivative, and observing that the optimal solution must lie 
in the interval B 8 = [— 1 , 1 ]. 


For the second factor of the right-end side of this expression 
we have 


E |cos 2 (^ fc )| = E | i i cos(2^fe) 


— 2 + 2 ® { cos (2 (^fc + 1110,1:))} 

= \ + \ E ^ C ° S C ° S ( 2u ^’ fc ) 

- sin (2 (f> k ) sin ( 2 w^ k )} 

+ 7 } cos ( 2 M E {cos ( 2 ui 0 > fc)} . (56) 


Using the cosine series, we write 


f w 2 3w 4 

E {cos(2u; 0)fc )} =E { l-2 2 ^ + 2 4 


4! 


. 2 ^ + ... +Hr M" 


(2 n)\ 


2 4 6 

2 CT 0 O 3 CT 0 


=1 _ 2 — + 2 Z — ! - - 2 ' 

1! 2! 3! 


+ ••• + (- 1 )’ 


2"crj 


= e" 2 ^ . (57) 


By substituting (57) in (56), we obtain: 


E |cos 2 (0 fe )| = | + ^cos(20 fc )e 2 ^ . (58) 


Eq. (19) follows by substituting (58) in (55), therefore con¬ 
cluding the proof. 


Appendix F 
Proof of Lemma 4.1 

We note that the speed at time k can be written as 14 = Vo + 
113,1 + ■ • ■ + f 3 ,fc-i • Since the process noise in the speed, V 3 , 
is white, it follows that 14 ~ A/" (Vb, (k — l)er|). Similarly, 
for the orientation <f> k , we have that f k ~ A/" {k — 1) cr 2 ). 
Then the Lemma follows by using Lemma 3.4, after simple 
algebraic and trigonometric calculations. 

Appendix G 

Proof of Proposition 4.2 

First, we start by proving the following initial lemma: 

Lemma G.l: Let a, b be two independent random variables 
with non-zero average. Suppose that 


lim JEifcJLSin =0 . 


Then, 


E 


e 


k—>oo 


E {a} 
E {b} 


(E{6}) b 


f+Et- 1 )* 


(59) 


E {(6- E {6}) fe } 


k=i (Ewr 

Proof: Since a and b are independent, we have that 


E 


{?} = EMe{1}. (60) 


Furthermore, 
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By using Taylor series expansion as in [48, Section 5-4], we variable Xz/Xq is distributed according to a doubly noncentral 
can write F-distribution. Then, we have that 


E 



E {b} ) J 


^ , E{(6- E{b}) k } 

h (E{ 6 }) fc 

(62) 


The sum in the previous equation converges as a consequence 
of assumption (59). Finally, by substituting (62) in (61) and 
(60), the lemma follows. ■ 

Now, using Lemma G. 1 above, we write the expectation as 



We are left with the derivation of E { z 2 /q 2 }. We start noting 
that, from Lemma G.l, we have 


E 






E 


fc=l 


E {z 2 } 
E {g 2 } 

(E{« 2 })‘ 


(64) 


The variable q 2 has a non-central chi-squared distribution of 
1 degree of freedom, thus E {g 2 } = <x 2 + fx 2 , and 


E 




=af E 


2 /c 

®q Mfe ■ 





This implies that 


lim 

k —>-oo 


E {(g 2 - E {g 2 })*} 


—2k .. 

,. a q l- L k 


= lim 

k—too 



= 0 , 


since ///,. is bounded and the denominator grows unbounded, 
and condition (59) holds, which is needed for the convergence 
of the sum in Eq. (64). Therefore, Eq. (64) becomes 


E 



OO 

i+y^(-i) fe 


^2k ,, 
Vq Vk 


fc= 1 {^q+ a q) k . 


■ ( 66 ) 


We are left with the computation of the expectation in the 
numerator of the summation in (63). This reduces to the k-th 
central moment of a doubly noncentral F-distribution. With 
this goal in mind, we first define % z = z 1 j(j\ and Xq = 
q 2 / a 2 . We note that Xz and \q are distributed according to a 
noncentral chi-squared distribution with 1 degree of freedom. 
Therefore z 2 /q 2 = Xz/Xq> since a 2 = a 2 , where the random 



The proposition follows by substituting ( 66 ) and (67) in 
(63), and noting that as required by (59), 



since the numerator is given by (67) and is bounded, whereas 
the denominator diverges because is the power of a number 
greater than 1 due to that the expectation in the denominator 
is positive. 


Appendix FI 

Proof of Proposition 4.3 

The upper bound follows immediately from the fact that 
q 2 /(q 2 + z 2 ) < 1, Vg G 1R, Vz G E. Regarding the lower 
bound, by noting that In (•) is a concave function and applying 
Jensen’s inequality, we obtain that 

e E{ln( g 2 )}-ln(E{ g 2 }+E{^}) < E j ^ 1 (68) 

{q 2 + z 2 ) 

Moreover, E{ln(g 2 )} = — j e — In (2) — 2 In (a q ) — 
M (0, -~n 2 /2a 2 ), whereas the proposition follows by sub¬ 

stituting the previous equation in (68). 

Appendix I 

Proof of Proposition 4.5 

We begin by observing that Vi, j the two matrices and 
jF 6 ) satisfy < J ^ . Consider the element-wise 

upper bound .L ” h> . It follows that there exists a matrix G 
such that J ij + Gjj > 3^ b ^\/i,j and J + G = where 

G is symmetric and diagonally dominant with real and non¬ 
negative elements. From the Gershgorin circle theorem, [49], it 
follows that all its eigenvalues are non-negative, i.e., A(G) > 
0. Thus, A (j(“ b ’ G )) = A (J) + A (G) because both J and G 
are Hermitian. Therefore, since J is positive semidefinite and 
A (J(“ 6 ’ G )) > A (J), J A j( uf> ’ G ) and 0 A j(“ 6 > G ). The proof 
is concluded by noting that the same steps may be employed 
for the lower bound j( ib - G ). 
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